#标准三维Astar算法
import time
import heapq
import numpy as np
#import gymnasium as gym
#from gymnasium import spaces
import matplotlib.pyplot as plt
import matplotlib.colors as mcolors
#import route_fy_v2.envs.createMap_v2 as cm
#import route_fy_v2.envs.drawmap as dm
<<<<<<< HEAD
=======
import JudeI 
>>>>>>> 04ecece5279dfc4603ce987a11d5932146f4f394

#def getmap():
#    map=cm.createmap(map_shape=(3,224, 224),
#                     num_flange=1,
#                     num_big=1,num_pot=5,num_apt=1,
#                     num_clamp=5,
#                     relist=["区域1","区域8","区域5","区域4","区域2"])
#    #cm.drawmap(map.map)
#    return map

class points:
<<<<<<< HEAD
    def __init__(self,x,y,z,prepoint=None):
        self.x=x
        self.y=y
        self.z=z
=======
    def __init__(self,x,y,z,prepoint=None,p=None,th=None):
        self.x=x
        self.y=y
        self.z=z
        self.p=p
        self.th=th
        self.zh=x
>>>>>>> 04ecece5279dfc4603ce987a11d5932146f4f394
        self.cost=np.inf
        self.g=np.inf
        self.h=np.inf
        self.prepoint=prepoint
        self.position=np.array((self.x,self.y,self.z),dtype=np.int16)
        if self.prepoint==None:
            self.v=np.array([0,0,0])
        else:
            self.v=(np.array(self.prepoint.position) - np.array(self.position))/np.linalg.norm(np.array(self.prepoint.position) - np.array(self.position))
        

    def __lt__(self, other):
        return self.cost < other.cost

class Astar:
    def __init__(self,map,start,end):
        self.map=map
        self.open_set=[]
        self.close_set=[]
        self.start=start
        self.end=end
        

        self.neighbourlist=[]
        for i in [-1,0,1]:
            for j in [-1,0,1]:
                for k in [-1,0,1]:
                    if j==0 and k==0 and i==0 :
                        continue

                    self.neighbourlist.append((i,j,k))
        #for j in [-1,0,1]:
        #    for k in [-1,0,1]:
        #        if j==0 and k==0 :
        #            continue
        #        self.neighbourlist.append((0,j,k))
        #for i in [-1,1]:
        #    self.neighbourlist.append((i,0,0))        


    def Cost(self,p):
        if p.prepoint  is None:
            g=0
        else:
            g=p.prepoint.g+np.linalg.norm(np.array(p.position) - np.array(p.prepoint.position)
                                          ,ord=1)
            if any(p.v!=p.prepoint.v):
                g+=1
        g+=0.0*p.y
        h=self.Heuristic(p=p)
        cost=g+1.0*h
        p.g=g
        p.h=h
        p.cost=cost
        return p
    
    def recost(self,p,f):
        if p.prepoint  is None:
            g=0
            h=self.Heuristic(p=p)
            cost=g+1.0*h
            p.g=g
            p.h=h
            p.cost=cost
            p.prepoint=f
        else:
            g=f.g+np.linalg.norm(np.array(p.position) - np.array(f.position),ord=1)
            g+=0.0*p.y
            if any(p.v!=p.prepoint.v):
                g+=1
            if g<p.g:
                p.g=g
                p.h=self.Heuristic(p=p)
                p.cost=g+1.0*p.h
                p.prepoint=f
            else:
                pass            
        return p
        
    def Heuristic(self,p):
        h=np.linalg.norm(np.array(self.end) - np.array(p.position),ord=1)
        return h
    def discane(self,p):
        h=np.linalg.norm(np.array(self.end) - np.array(p.position),ord=1)
        #step=1
        if h>45:
<<<<<<< HEAD
            step=5
        elif h>10:
            step=3
=======
            step=10
        elif h>8:
            step=5
>>>>>>> 04ecece5279dfc4603ce987a11d5932146f4f394
        else:
            step=1
            
        return step

    def FindPath(self):
        num=0
        startpoints=points(self.start[0],self.start[1],self.start[2])
        startpoints=self.Cost(startpoints)
        self.open_set.append(startpoints)
        while len(self.open_set)>0:
            num+=1
            self.open_set.sort(key=lambda x:x.cost)
            current=self.open_set[0]
            #print(self.open_set[0].position)
            #print(current.x,current.y,current.z)

            step=self.discane(current)
            neighbour=self.getneighbour(current,step=step)

            if (current.x>=self.map.shape[0] or current.y>=self.map.shape[1] or current.z>=self.map.shape[2]):
                print('out of map',current.x,current.y,current.z,map.shape)
                print(current.position,current.prepoint.position)
                break
            #print(current.x,current.y,current.z)
            #print(current.x,current.y,current.z, end='\r')
            for i in neighbour: 
                if any(np.all(i.position == x.position) for x in self.open_set) :
                    self.recost(i,current)
                    continue
                elif any(np.all(i.position == x.position) for x in self.close_set) :
                    continue
                else:
                    i= self.Cost(i)
                    self.open_set.append(i)
                    if np.all(i.position==np.array(self.end,dtype=np.int16)):
                        return True,i
            self.close_set.append(current)                
            self.open_set.pop(0)
            if num>1e6:
                return False,None
        return False,None
    
    def heuristic_cost_estimate(self, a, b):
        return np.linalg.norm(np.array(a.position) - np.array(b.position))


    def find_path(self):
        start_point = points(self.start[0], self.start[1], self.start[2])
        start_point = self.Cost(start_point)
        heapq.heappush(self.open_set, (start_point.cost, start_point))

        while self.open_set:
            current = heapq.heappop(self.open_set)[1]

            if np.all(current.position == np.array(self.end, dtype=np.int16)):
                return True, current

            self.close_set.append(current)

            for neighbor in self.getneighbour(current):
                if neighbor in self.close_set:
                    continue

                tentative_g_score = current.cost + self.heuristic_cost_estimate(current, neighbor)

                if neighbor not in [item[1] for item in self.open_set] or tentative_g_score < neighbor.cost:
                    neighbor.cost = tentative_g_score
                    heapq.heappush(self.open_set, (tentative_g_score, neighbor))

        return False, None
    
    def getneighbour(self,p,step=1):
        neighbour=[]
        for i in self.neighbourlist:
            if np.all(i==[0,0,0]):
               continue
            else:
<<<<<<< HEAD
=======
                #x= p.position[0]+i[0]*step
                #y= p.position[1]+i[1]*step
                #z= p.position[2]+i[2]*step
>>>>>>> 04ecece5279dfc4603ce987a11d5932146f4f394
                x= p.position[0]+i[0]*step
                y= p.position[1]+i[1]*step
                z= p.position[2]+i[2]*step
                if x>=0 and x<self.map.shape[0] and y>=0 and y<self.map.shape[1] and z>=0 and z<self.map.shape[2]:
                    #if self.map[x,y,z]==0:
                    #    npoint=points(x,y,z,p)
                    #    neighbour.append(npoint)
                    inc,_=line_tool(self.map,p.position,np.array([x,y,z]),b=1,h=1,lab=75)
                    if inc==0:
                       if self.map[x,y,z] in [20,0,75,240,220]:
                            npoint=points(x,y,z,p)
                            neighbour.append(npoint)
<<<<<<< HEAD
        if step<2:
            inc,_=line_tool(self.map,p.position,np.array(self.end),b=1,h=1,lab=75)
            npoint=points(self.end[0],self.end[1],self.end[2],p)
            neighbour.append(npoint)
=======
        if step<=3:
            inc,_=line_tool(self.map,p.position,np.array(self.end),b=1,h=1,lab=75)
            if inc==0:
                if self.map[x,y,z] in [20,0,75,240,220]:
                   npoint=points(self.end[0],self.end[1],self.end[2],p)
                   neighbour.append(npoint)
>>>>>>> 04ecece5279dfc4603ce987a11d5932146f4f394
                    
                    
        #if step>3:
        #    for t in [1,2,3]:
        #        for i in self.neighbourlist:
        #            if np.all(i==[0,0,0]):
        #               continue
        #            else:
        #                x= p.position[0]+i[0]*(1)
        #                y= p.position[1]+i[1]*(step+t)
        #                z= p.position[2]+i[2]*(step+t)
        #                if x>=0 and x<self.map.shape[0] and y>=0 and y<self.map.shape[1] and z>=0 and z<self.map.shape[2]:
        #                     inc,_=line_tool(self.map,p.position,np.array([x,y,z]),b=1,h=1,lab=75)
        #                     if inc==0:
        #                        if self.map[x,y,z] in [20,0,75,240,220]:
        #                             npoint=points(x,y,z,p)
        #                             neighbour.append(npoint)
        return neighbour


def line_tool(M,p1,p2,b,h,lab,pass_obj_list=None):
    interf=0
    throgh=0
    n_x=int((np.abs(p2[0] - p1[0])+1) / b)
    n_y=int((np.abs(p2[1] - p1[1])+1) / b)
    n_z=int((np.abs(p2[2] - p1[2])+1) / b)
    n=max(n_x,n_y,n_z)
    x=np.round(np.linspace(p1[0], p2[0], n), 0).astype(int)
    y=np.round(np.linspace(p1[1], p2[1], n), 0).astype(int)
    z=np.round(np.linspace(p1[2], p2[2], n), 0).astype(int)
    j1=[]
    j2=[]
    for dq in [-1, 0, 1]:
        for dr in [-1, 0, 1]:
            if(dq==0 and dr!=0) or (dr==0 and dq!=0):
                j1.append([p1[0], p1[1] + dq, p1[2]+ dr])

    for i,j,k in zip(x,y,z):
        
        if all([i,j,k]==p1):
            
            continue
        if all([i,j,k]==p2):
           
            continue
        if i>=M.shape[0] or j>=M.shape[1] or k>=M.shape[2]:
            interf+=1
            continue
        elif i<0 or j<0 or k<0:
            interf+=1
            continue
        #elif [i,j,k] in j1:
        #    #M[int(i),int(j),int(k)]=175
        #    continue
        elif M[int(i),int(j),int(k)] in [20,0,75,240,220]:
           pass
        elif M[int(i),int(j),int(k)]==0:
            throgh+=1
        elif M[int(i),int(j),int(k)]==lab:
            throgh+=1
        else:#干涉
            point=[i,j,k]
            if pass_obj_list==None:
                interf+=1
            #else:
            #    for obj in pass_obj_list:
            #        if point_in_AABB(point, obj.box.min, obj.box.max):
            #            continue
            #        else:
            #            interf+=1
    
    #j1.clear()
    #for dq in [-1, 0, 1]:
    #    for dr in [-1, 0, 1]:
    #        if(dq==0 and dr!=0) or (dr==0 and dq!=0):
    #            j2.append([p1[0], p1[1] + dq, p1[2]+ dr])
    ##j1.append([p1[0], p1[1], p1[2]])
    #  
    #for i,j,k in zip(x,y,z):
    #    if   all([i,j,k]==p2) :
    #        #print(1)
    #        continue
    #    else:
    #        for dq in [-1, 0, 1]:
    #            for dr in [-1, 0, 1]:
    #                    # 排除当前点本身
    #                #if  all([i,j,k]==p1):
    #                #    j2.append([[i,j,k]])
    #                if (dq, dr) == (0, 0) :
    #                        j2.append([i,j,k])
    #                        continue
    #                    # 获取当前点的值
    #                elif(dq==0 and dr!=0) or (dr==0 and dq!=0):
    #                    if i>=3 or j+ dq>=224 or k+ dr>=224:
    #                        interf+=1
    #                        continue
    #                    elif [i , j+ dq, k+ dr] in j2:
    #                        if M[i , j+ dq, k+ dr ]==20:
    #                            M[i , j+ dq, k+ dr ] = 160
    #                        continue
    #                
    #                    if M[i , j+ dq, k+ dr ]==20:
    #                        M[i , j+ dq, k+ dr ] = 160
    #                        j2.append([i , j+ dq, k+ dr ])
    #                    elif M[i , j+ dq, k + dr]==lab:
    #                        #M[i , j+ dq, k + dr]=160
    #                        j2.append([i , j+ dq, k + dr])
    #                        #throgh+=1
    #                    else:
    #                        #干涉
    #                        if (i, j + dq, k+ dr) in zip(x, y, z):
    #                            pass
    #                        else:
    #                            interf+=1
    #                        continue
    #        j1=j2
    return interf,throgh

#def test():
#    map=getmap()
#    s=map.starpot[0].getcod()
#    e=map.endpot[0].getcod()
#    print(s,e)
#    #利用time计时
#    start=time.time()
#    A=Astar(map=map,start=s,end=e)
#    result,info=A.FindPath()
#    end=time.time()
#    print("time:",end-start)
#    print()
#    print(result,info)
#    father=False
#    centerlist=[]
#    openlist=[]
#    closelist=[]
#    while father==False:
#        centerlist.append(info.position)
#        info=info.prepoint
#        father=(info.prepoint is None)
#
#    for i in A.open_set:
#        openlist.append(i.position)
#        
#    for i in A.close_set:
#        closelist.append(i.position)
#
#    result_map=map.map
#    
#    for i in openlist:
#        result_map[i[0],i[1],i[2]]=150
#    for i in closelist:
#        result_map[i[0],i[1],i[2]]=0
#    for i in centerlist:
#        result_map[i[0],i[1],i[2]]=50
#    #cm.drawmap(result_map)
#    p=dm.P_show()
#    p.draw(result_map,None)
#    p.result()


#test()


